///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_CV_CCLOUD_H_
#define _BUOLA_CV_CCLOUD_H_

#include <buola/mat.h>
#include <buola/mat/cquat.h>
#include <buola/mat/transformations.h>
#include <buola/image/format.h>
#include <buola/cv/points.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/io/curi.h>

namespace buola { namespace cvi {

template<typename tPoint>
class CCloud : private UStorage<tPoint,cDynamic>
{
public:
    typedef tPoint TPoint;
    typedef typename TPoint::TScalar TScalar;
    typedef typename TPoint::TVec TVec;
    typedef mat::CQuat<TScalar> TQuat;
    typedef UStorage<TPoint,cDynamic> TBase;

public:    
    ///default constructor
    CCloud(const TVec &pOrigin=nZero,const TQuat &pRotation=TQuat())
        :   mSize(0)
        ,   mOrigin(pOrigin)
        ,   mRotation(pRotation)
    {}

    ///move constructor
    CCloud(CCloud&&)=default;
    CCloud(const CCloud&)=delete;
    
    CCloud(NClone,const CCloud &pO)
        :   TBase(pO.mSize)
        ,   mSize(pO.mSize)
        ,   mOrigin(pO.mOrigin)
        ,   mRotation(pO.mRotation)
    {
        copy_array(pO.Data(),this->Data(),mSize);
    }
    
    CCloud(const CCloud &pO,const std::vector<int> &pIndices)
        :   TBase(pIndices.size())
        ,   mSize(pIndices.size())
        ,   mOrigin(pO.mOrigin)
        ,   mRotation(pO.mRotation)
    {
        for(int i=0;i<pIndices.size();i++)
            this->At(i)=pO.mPoints[pIndices[i]];
    }
    
    explicit CCloud(const img::CView_data<tPoint> &pView);
    
    ///destructor
    ~CCloud()=default;

    ///returns the origin for the viewpoint
    const TVec &Origin() const  {   return mOrigin;    }

    ///returns the origin for the viewpoint
    const TQuat &Rotation() const  {   return mRotation;    }

    ///sets the origin of the viewpoint
    void SetOrigin(const TVec &pOrigin)    {   mOrigin=pOrigin;    }
    
    ///sets the origin of the viewpoint
    void SetRotation(const TQuat &pRotation)    {   mRotation=pRotation;    }

    CCloud &operator=(const CCloud&)=delete;
    CCloud &operator=(CCloud&&)=default;
    
    void reserve(size_t pCapacity)
    {
        if(pCapacity>this->Capacity())
            this->Resize(pCapacity,mSize);
    }
    
    void resize(size_t pCapacity)
    {
        reserve(pCapacity);
        mSize=pCapacity;
    }
    
    void push_back(const tPoint &pPoint)
    {
        if(mSize>=this->Capacity())
            this->Resize(std::max(2*mSize,mSize+16),mSize);
        new(&this->At(mSize++)) tPoint(pPoint);
    }
    
    template<typename... tArgs>
    void emplace_back(tArgs&&... pArgs)
    {
        if(mSize>=this->Capacity())
            this->Resize(std::max(2*mSize,mSize+16),mSize);
        new(&this->At(mSize++)) tPoint(std::forward<tArgs>(pArgs)...);
    }
    
    size_t size() const {   return mSize;   }
    
    const tPoint *data() const      {   return this->Data();        }
    const tPoint *begin() const     {   return this->Data();        }
    const tPoint *end() const       {   return this->Data()+mSize;  }
    
    tPoint *data()      {   return this->Data();        }
    tPoint *begin()     {   return this->Data();        }
    tPoint *end()       {   return this->Data()+mSize;  }
    
    tPoint &operator[](size_t i)                {   return this->Data()[i];   }
    const tPoint &operator[](size_t i) const    {   return this->Data()[i];   }
    
private:
    size_t mSize;
    TVec mOrigin;
    TQuat mRotation;
};

typedef CCloud<CCloudPoint_rgb8> CCloud_rgb8;
typedef CCloud<CCloudPoint_f> CCloud_f;
typedef CCloud<CCloudPoint_n_rgb8> CCloud_n_rgb8;
typedef CCloud<CCloudPoint_n_f> CCloud_n_f;

CCloud_rgb8 load_cloud(const io::CURI &pURI);
void save_cloud(const CCloud_rgb8 &pCloud,const io::CURI &pURI);
void save_cloud_pcd(const CCloud_rgb8 &pCloud,const io::CURI &pURI);
void save_cloud(const CCloud_f &pCloud,const io::CURI &pURI);
void save_cloud_pcd(const CCloud_f &pCloud,const io::CURI &pURI);
CCloud_rgb8 load_cloud_pcd(const io::CURI &pURI);
CCloud_rgb8 load_cloud(const io::CURI &pDepth,const io::CURI &pImage,const io::CURI &pInfo);
CCloud_rgb8 make_cloud(const img::CView_xyz32f &pPos,const img::CView_rgb8 &pImage,const mat::CVec3f &pCamPos=mat::CVec3f(0,0,0));
CCloud_rgb8 make_cloud(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pImage,const img::CCameraParameters &pParams);
CCloud_rgb8 make_cloud(const img::CView_gray32f &pDepth,const img::CColor &pColor,const img::CCameraParameters &pParams);
CCloud_n_rgb8 make_cloud_with_normals(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pImage,
                                    const img::CCameraParameters &pParams,int pRadius);
CImage_point_rgb8 make_point_image(const img::CView_gray32f &pDepth,const img::CView_rgb8 &pRGB,
                                        const img::CCameraParameters &pCamera);
img::CImage_xyz32f make_pos_image(const img::CView_gray32f &pDepth,const img::CCameraParameters &pCamera);

template<typename tPoint>
CCloud<tPoint>::CCloud(const img::CView_data<tPoint> &pView)
    :   TBase(pView.Width()*pView.Height())
{
    size_t i=0;
    for(int y=0;y<pView.Height();y++)
    {
        for(int x=0;x<pView.Width();x++)
        {
            const auto &lPixel=pView(x,y);
            if(!is_nan(lPixel->mPoint.x()))
                this->At(i++)=lPixel;
        }
    }
    mSize=i;
}

template<typename tPoint>
typename CCloud<tPoint>::TVec center(const CCloud<tPoint> &pCloud)
{
    typename CCloud<tPoint>::TVec lCenter(nZero);
    for(auto &lPoint : pCloud)
        lCenter+=lPoint.mPoint;
    return lCenter/pCloud.size();
}

template<typename tPoint>
typename CCloud<tPoint>::TVec min(const CCloud<tPoint> &pCloud)
{
    typename CCloud<tPoint>::TVec lMin({INFINITY,INFINITY,INFINITY});
    for(auto &lPoint : pCloud)
    {
        lMin.x()=std::min(lMin.x(),lPoint.mPoint.x());
        lMin.y()=std::min(lMin.y(),lPoint.mPoint.y());
        lMin.z()=std::min(lMin.z(),lPoint.mPoint.z());
    }    
    return lMin;
}

template<typename tPoint>
typename CCloud<tPoint>::TVec max(const CCloud<tPoint> &pCloud)
{
    typename CCloud<tPoint>::TVec lMax({-INFINITY,-INFINITY,-INFINITY});
    for(auto &lPoint : pCloud)
    {
        lMax.x()=std::max(lMax.x(),lPoint.mPoint.x());
        lMax.y()=std::max(lMax.y(),lPoint.mPoint.y());
        lMax.z()=std::max(lMax.z(),lPoint.mPoint.z());
    }
    return lMax;
}

template<typename tPoint>
CCloud<tPoint> transform(const CCloud<tPoint> &pCloud,const mat::CMat34f &pTrans)
{
    CCloud<tPoint> lCloud=clone(pCloud);
    
    for(auto &p : lCloud)
    {
        p.mPoint=pTrans*p.mPoint;
    }
    
    return lCloud;
}

template<typename tPoint>
CCloud<tPoint> scale_cloud(const CCloud<tPoint> &pCloud,float pFactor)
{
    CCloud<tPoint> lCloud=clone(pCloud);
    
    for(auto &p : lCloud)
    {
        p.mPoint=pFactor*p.mPoint;
    }
    
    return lCloud;
}

template<typename tPoint>
CCloud<tPoint> transform_lookat(const CCloud<tPoint> &pCloud,const mat::CVec3f &pAt,
                                         const mat::CVec3f &pEye,const mat::CVec3f &pUp)
{
    mat::CVec3f lFront=pAt-pEye;
    lFront/=norm(lFront);
    mat::CVec3f lRight=cross(lFront,pUp);
    lRight/=norm(lRight);
    mat::CVec3f lUp=cross(lRight,lFront);

    mat::CMat34f lTransform{lRight.x(),lRight.y(),lRight.z(),-dot(lRight,pEye),
            -lUp.x(),-lUp.y(),-lUp.z(),dot(lUp,pEye),
            lFront.x(),lFront.y(),lFront.z(),-dot(lFront,pEye)};
    return transform(pCloud,inverse(lTransform));
}

template<typename tPoint>
CCloud<tPoint> downsample(const CCloud<tPoint> &pCloud,size_t pFactor)
{
    CCloud<tPoint> lCloud;
    lCloud.reserve(div_round_up(pCloud.size(),pFactor));
    for(int i=0;i<pCloud.size();i+=pFactor)
    {
        lCloud.emplace_back(pCloud[i]);
    }
    
    return lCloud;
}

template<typename tPoint,typename tFunc>
CCloud<tPoint> filter(const CCloud<tPoint> &pCloud,tFunc pFunc)
{
    CCloud<tPoint> lCloud;
    for(int i=0;i<pCloud.size();i++)
    {
        if(pFunc(pCloud[i]))
            lCloud.emplace_back(pCloud[i]);
    }
    
    return lCloud;
}

template<typename tSerializer,typename tPoint>
void serialize(tSerializer &pS,const CCloud<tPoint> &pC)
{
    pS << pC.size();
    pS << CRange<const uint8_t*>((const uint8_t*)pC.data(),(const uint8_t*)(pC.data()+pC.size()));
}

template<typename tSerializer,typename tPoint>
void deserialize(tSerializer &pS,CCloud<tPoint> &pC)
{
    size_t lSize;
    pS >> lSize;
    pC.resize(lSize);
    pS >> CRange<uint8_t*>((uint8_t*)pC.data(),(uint8_t*)(pC.data()+pC.size()));
}

/*namespace cvi*/ } /*namespace buola*/ }

#endif
